
#include <stdint.h>
#include <stdio.h>

#include "hpm_clock_drv.h"
#include "hpm_mchtmr_drv.h"
#include "board.h"

#include "jtag.h"
#include "riscv_debug.h"
#include "target.h"
#include "flash/flash.h"

#include "led_demo_flash.h" 
#include "led_demo_ram.h"

target_t target_riscv;
char w_mem[32];
char r_mem[32];
uint64_t w_fp[32];
uint64_t r_fp[32];

uint64_t st, et, freq;
void test(void)
{
    // ms * freq / 1000
    freq = clock_get_frequency(clock_mchtmr0);
    
//  init test
    target_riscv_init(&target_riscv);
    target_riscv.init();
    if (irlen != 5)
    {
        RISCV_DEBUG_LOG_DBG("ERR: irlen = 0x%x \n", irlen);
        return;
    }
    if (tapnum != 1)
    {
        RISCV_DEBUG_LOG_DBG("ERR: tapnum = 0x%x \n", tapnum);
        return;
    }
    if (idcode != 0x1000563D)
    {
        RISCV_DEBUG_LOG_DBG("ERR: idcode = 0x%x \n", idcode);
        return;
    }
    target_riscv.idcode = idcode;
#if 0
//  memory access test
    for (int i = 0; i < 32; i++)
    {
        w_mem[i] = i + 2;
    }
    if (!target_riscv.write_mem(0x3, w_mem, 32))
    {
        if (!target_riscv.read_mem(0x3, r_mem, 32))
        {
            for (int i = 0; i < 32; i++)
            {
                if (w_mem[i] != r_mem[i])
                {
                    RISCV_DEBUG_LOG_DBG("write/read memory err! \n");
                    goto err;
                }
            }
        }
    }

//  bkt resume test
//  led main fun(0x80005ae4)
    uint32_t PC = 0x80005ae4;
    uint32_t PC2 = 0;
    uint32_t PC3 = 0;
    uint32_t PC4 = 0;
    int reason = -1;
    int reason2 = -1;
    int stat = -1;

    target_riscv.read_reg(&PC2, 32, 4);
    stat = target_riscv.resume(NULL, 0);
    asm volatile("ebreak");
    target_riscv.halt();
    asm volatile("ebreak");
    reason = target_riscv.reset(0); // 复位
    asm volatile("ebreak");
    reason2 = target_riscv.reset(1); // 复位后挂起
    asm volatile("ebreak");
    target_riscv.read_reg(&PC3, 32, 4);
    asm volatile("ebreak");
    stat = target_riscv.breakpiont(&PC, 0, 1); // 设置硬件断点
    asm volatile("ebreak");
    stat = target_riscv.resume(NULL, 0);
    asm volatile("ebreak");
    //target_riscv.halt();
    //asm volatile("ebreak");
    target_riscv.read_reg(&PC4, 32, 4);
    asm volatile("ebreak");

    stat = target_riscv.breakpiont(&PC, 0, 0); 
    asm volatile("ebreak");

//  step test
    uint32_t PC5 = 0;
    uint32_t PC6 = 0;
    uint32_t PC7 = 0;
    stat = target_riscv.step(1); // 进入单步模式
    //asm volatile("ebreak");
    stat = target_riscv.resume(NULL, 0);
    //asm volatile("ebreak");
    stat = target_riscv.read_reg(&PC5, 32, 4);
    asm volatile("ebreak");
    stat = target_riscv.resume(NULL, 0);
    //asm volatile("ebreak");
    stat = target_riscv.read_reg(&PC6, 32, 4);
    asm volatile("ebreak");
    stat = target_riscv.resume(NULL, 0);
    //asm volatile("ebreak");
    stat = target_riscv.read_reg(&PC7, 32, 4);
    asm volatile("ebreak");
    stat = target_riscv.step(0); // 退出单步模式
    stat = target_riscv.resume(NULL, 0);
    asm volatile("ebreak");
//  Floating-point register 
    target_riscv.halt();
    asm volatile("ebreak");
    uint32_t m = 0x2000UL;
    target_riscv.write_reg(&m, CSR_MSTATUS, 4);
    uint32_t f = 0;
    target_riscv.write_reg(&f, 68, 4); // fcsr
    
    uint64_t f0 = 0xaa55aa5555aa5500;
    for (int i = 0; i < 32; i++)
    {
        target_riscv.write_reg(&f0, 33 + i, 8);
        f0 ++;
    }
    for (int j = 0; j < 32; j++)
    {
        target_riscv.read_reg(&r_fp[j], 33 + j, 8);
    }
    asm volatile("ebreak");
//  flash algo test
    target_riscv.reset(1);
    asm volatile("ebreak");
    target_riscv.resume(NULL, 0);
    asm volatile("ebreak");
    target_riscv.halt();

    riscv_csr_dcsr_t dcsr;
    target_riscv.read_reg(&dcsr.reg, CSR_DCSR, 4);
    dcsr.ebreakm = 1;
    dcsr.ebreaks = 1;
    dcsr.ebreaku = 1;
    target_riscv.write_reg(&dcsr.reg, CSR_DCSR, 4);
     
    flash_init();
    flash_erase(0, 0x10000);
    flash_program(0x80000400 - 0x80000000, led_demo_flash, sizeof(led_demo_flash));
    target_riscv.reset(0);
#endif
//  
#if 0
    //target_riscv.halt();
    target_riscv.reset(1);
    clock_cpu_delay_ms(10);

    uint32_t val = 1;
    //riscv_debug_sysbus_write_mem(0xF4002000, &val, 4);
    target_riscv.write_mem(0xF4002000, &val, 4);
    clock_cpu_delay_ms(10);
    val = 2;
    riscv_debug_sysbus_write_mem(0xF4002000, &val, 4);
    val = 0xFFFFFFFF;
    riscv_debug_sysbus_write_mem(0xF4000800, &val, 4);
    riscv_debug_sysbus_write_mem(0xF4000810, &val, 4);
    riscv_debug_sysbus_write_mem(0xF4000820, &val, 4);
    riscv_debug_sysbus_write_mem(0xF4000830, &val, 4);

    // asm volatile("ebreak"); 
    st = mchtmr_get_count(HPM_MCHTMR);
    //target_riscv.write_mem(0, led_demo_ram, sizeof(led_demo_ram));
    riscv_debug_sysbus_write_mem(0, led_demo_ram, sizeof(led_demo_ram));
    et = mchtmr_get_count(HPM_MCHTMR);
    RISCV_DEBUG_LOG_DBG("%lld \n", (et - st) * 1000 / freq);
    
    asm volatile("ebreak");
    target_riscv.resume(0x0, 0);
    asm volatile("ebreak");
#endif
#if 0
    target_riscv.reset(0);
    asm volatile("ebreak");
    target_riscv.reset(1);
    asm volatile("ebreak");
    target_riscv.resume(NULL, 0);
    asm volatile("ebreak");
#endif
    riscv_csr_dcsr_t dcsr;
    target_riscv.read_reg(&dcsr.reg, CSR_DCSR, 4);
    dcsr.ebreakm = 1;
    dcsr.ebreaks = 1;
    dcsr.ebreaku = 1;
    target_riscv.write_reg(&dcsr.reg, CSR_DCSR, 4);

    asm volatile("ebreak");
    st = mchtmr_get_count(HPM_MCHTMR);
    riscv_debug_sysbus_write_mem(1, led_demo_ram, sizeof(led_demo_ram) - 1, 1);
    et = mchtmr_get_count(HPM_MCHTMR);
    RISCV_DEBUG_LOG_DBG("%lld(ms) \n", (et - st) * 1000 / freq);

    st = mchtmr_get_count(HPM_MCHTMR);
    riscv_debug_sysbus_write_mem(0, led_demo_ram, sizeof(led_demo_ram), 0);
    et = mchtmr_get_count(HPM_MCHTMR);
    RISCV_DEBUG_LOG_DBG("%lld(ms) \n", (et - st) * 1000 / freq);

    //target_riscv.read_mem(0, r_mem, 32);
    asm volatile("ebreak");
err:
    while (1){;}
}
